/*
rs57.c
offer leisai cl2c series , rs57 mode driver drive function realize
by lunanting


io control move point config
    route0      0
    route1      1
    route2      2
    route3      back zero

-------------------------
2021-03-30 16:02:19
creat file
-------------------------
*/
#include "rs57.h"
#include "../../bsp/cli_cmd.h"
#include "stdlib.h"
#include "../../bsp/io_config.h"

#ifndef RS57_max_num
#define RS57_max_num 10     //最多支持 rs57 电机数量
#endif

static int xp_rs57_debug(char *type,char *fun,char *param);
static char *xp_rs57_warn_string(int code);
void xp_rs57_status_check_thread(void *arg);

static RS57 *motor_list[RS57_max_num]={0};
static int list_foot=0;
static int mirrors_state = 0;//0-关闭状态，1-打开状态，2-中间角度
uint64_t g_last_mirrors_checkzero_time = 0;//反光镜上次回零的时间戳
uint64_t g_last_mirrors_close_time = 0;//反光镜上次关闭的时间戳

/*
rs57 new a object
param:
    port: modbus port
    speed: modbus speed
    id: driver modbus id
return:
    NULL= fail
*/
RS57 *xp_rs57_new(int port,int speed,int id){
    int sta=0;
    RS57 *new;
    RS57 obj={0};
    char buf[50];

    if(id<0||id>127){
        perr("id set error ,id = %d",id);
        return NULL;
    }
    obj.port=port;
    obj.speed=speed;
    obj.id=id;
    obj.structure.Kd=1.0;
    obj.structure.Kr=1.0;
    
    sta=xp_modbus_init(port,speed,MB_PAR_NONE,100);
    if(sta!=0){
        perr("rs57 %d motor modbus init fail, code=%d.",id,sta);
        println("rs57 %d motor only can use io control !",id);
    }
    obj.modbus=1;
    obj.init=1;
    new=(RS57 *)malloc(sizeof(RS57));
    memcpy(new,&obj,sizeof(RS57));
    if(list_foot<10){
        motor_list[list_foot++]=new;
    }
    else{
        perr("creat motor over %d number ! Can't create any more !",RS57_max_num);
        return NULL;
    }
    if(list_foot==1){
        xp_cli_debug_add(xp_rs57_debug);
    }
    sprintf(buf,"%d_rs57_status",obj.id);
    aos_task_new(buf,xp_rs57_status_check_thread,new,1024);
    println("rs57 driver init success ~ id = %d",id);
    return new;
}



/*
config si
param:
    motor:motor obj
    port:si io board id
    si1:si1 pin
    si2:si2 pin
    si3:si3 pin
    si4:si4 pin
    si5:si5 pin
    si6:si6 pin
    si7:si7 pin
return:
    0=success
*/
int xp_rs57_si_config(RS57 *motor,int id,int si[8]){
    if(motor==NULL){
        perr("motor address= NULL");
        return -1;
    }
    motor->si.id=id;
    // motor->si.si1_org=si[1];
    motor->si.si2_stop=si[2];
    motor->si.si3_en=si[3];
    motor->si.si4_clr=si[4];
    motor->si.si5_addr0=si[5];
    motor->si.si6_addr1=si[6];
    motor->si.si7_ctrg=si[7];
    return 0;
}

/*
config so
param:
    motor:motor obj
    id:so io board id
    so1:so1 pin cmd ok
    so2:so2 pin mc ok
    so3:sp3 pin alm
return:
    0=success
*/
int xp_rs57_so_config(RS57 *motor,int id,int so1,int so2,int so3){
    if(motor==NULL){
        perr("motor address = NULL");
        return -1;
    }
    motor->so.id=id;
    motor->so.so1_mc_ok=so1;
    motor->so.so2_brk=so2;
    motor->so.so3_alm=so3;
    return 0;
}




/*
get motor object from modbus id
param:
    id: modbus id
return: motor object pointer
*/
RS57 *xp_rs57_motor_get(int id){
    for(int i=0; i<list_foot; i++){
        if(motor_list[i]==NULL){
            return NULL;
        }
        if(motor_list[i]->id==id){
            return motor_list[i];
        }
    }
    return NULL;
}


/*
config motor holding brake io
param:
    motor:motor object
    id: io board id
    pin: io out pin number
    touch: io touch status
return:
    0=success
*/
int xp_rs57_hold_config(RS57 *motor,int id,int pin,int touch){
    if(motor==NULL){
        perr("rs57 input motor object == NULL.");
        return -1;
    }
    motor->hold.id=id;
    motor->hold.pin=pin;
    motor->hold.touch=touch;
    motor->hold.status=0;
    println("rs57 %d motor hold config success.",motor->id);
    return 0;
}


/*
config motor limit check io
param:
    motor: motor obj
    id: board id
    max_pin: max pin io number
    min_pin: min pin io number
*/
int xp_rs57_limit_io_config(RS57 *motor, int id,int max_pin,int min_pin){
    if(motor==NULL){
        perr("rs57 motor object == NULL.");
        return -1;
    }
    motor->limit.id=id;
    motor->limit.max=max_pin;
    motor->limit.min=min_pin;
    println("rs57 %d motor limit io config success.",motor->id);
    return 0;
}

/**********************************io control motor**********************************/
/*
enable motor
param:
    motor: motor object
    sta: enable state,0=disable,1=enable
return:
    0=success
*/
int xp_rs57_en_set(RS57 *motor,int sta){
    if(motor==NULL){
        perr("motor == null");
        return -2;
    }
    if(motor->si.id==0){
        perr("rs57 id=%d ,si unconfig !",motor->id);
        return -1;
    }
    if(sta==0){
        println("rs57 %d motor disable !",motor->id);
        xp_io_write_pin(motor->si.id,motor->si.si3_en,0);
    }
    else{
        println("rs57 %d motor enable !",motor->id);
        xp_rs57_clear(motor);
        xp_io_write_pin(motor->si.id,motor->si.si3_en,1);
    }
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,1);
    return 0;
}

/*
set run route
param:
    motor: motor object
    num: address num
return:
    0=success
*/
int xp_rs57_route_set(RS57 *motor,int num){
    if(motor==NULL){
        perr("motor == null");
        return -1;
    }
    if(num>3||num<0){
        perr("route address num set error ! %d motor num=%d",motor->id,num);
        return -2;
    }
    println("rs57 %d set run route: %d",motor->id,num);
    if(num==3){
        motor->dir=-2;
        if(motor->limit.status==-1||motor->limit.status==-2){
            perr("rs57 motor %d min limit touched !",motor->id);
            return -4;
        }
    }
    else if(num>motor->route){
        motor->dir=1;
        if(motor->limit.status==1||motor->limit.status==-2){
            perr("rs57 motor %d max limit touched !",motor->id);
            return -3;
        }
    }
    else if(num<motor->route){
        motor->dir=-1;
        if(motor->limit.status==-1||motor->limit.status==-2){
            perr("rs57 motor %d min limit touched !",motor->id);
            return -4;
        }
    }
    else if(num==motor->route){
        motor->dir=0;
    }
    switch (num)
    {
    case 0:
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,1);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,1);
        xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
        break;
    case 1:
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,0);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,1);
        xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
        break;
    case 2:
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,1);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,0);
        xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
        break;
    case 3:
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,1);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,1);
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,0);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,0);
        xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
        break;
    default:
        xp_io_write_pin(motor->si.id,motor->si.si5_addr0,1);
        xp_io_write_pin(motor->si.id,motor->si.si6_addr1,1);
        xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
        break;
    }
    motor->route=num;
    if(num==3){
        motor->route=0;
    }
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,1);
    return 0;
}

/*
clear warn
param:
    motor: motor obj
return:
    0=success
*/
int xp_rs57_clear(RS57 *motor){
    if(motor==NULL){
        perr("motor == null");
        return -1;
    }
    xp_io_write_pin(motor->si.id,motor->si.si4_clr,0);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
    aos_msleep(100);
    xp_io_write_pin(motor->si.id,motor->si.si4_clr,1);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,1);
    return 0;
}


/*
set goto origin single
param:
    motor: motor obj
return:
    0=success
*/
int xp_rs57_origin_go(RS57 *motor){
    if(motor==NULL){
        perr("motor == null");
        return -1;
    }
    xp_io_write_pin(motor->si.id,motor->si.si1_org,0);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
    aos_msleep(100);
    xp_io_write_pin(motor->si.id,motor->si.si1_org,1);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,1);
    return 0;
}

/*
fast stop
param:
    motor: motor obj
return:
    0=success
*/
int xp_rs57_stop(RS57 *motor){
    if(motor==NULL){
        perr("motor == null");
        return -1;
    }
    xp_io_write_pin(motor->si.id,motor->si.si2_stop,0);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,0);
    aos_msleep(100);
    xp_io_write_pin(motor->si.id,motor->si.si2_stop,1);
    xp_io_write_pin(motor->si.id,motor->si.si7_ctrg,1);
    return 0;
}



/*
hold brake set
param:
    sta: 0=close,1=open
return:
    0=success
*/
int xp_rs57_hold_set(RS57 *motor,int sta){
    bool status=0;
    if(motor==NULL){
        perr("rs57 input object == NULL.");
        return -1;
    }
    if(sta==0){
        status=(~motor->hold.touch)&0x01;
    }
    else{
        status=motor->hold.touch&0x01;
    }
    if(motor->hold.id==0)return -2;
    xp_io_write_pin(motor->hold.id,motor->hold.pin,status);
    motor->hold.status=sta;
    println("rs57 %d motor hold set as: %d.",motor->id,sta);
    aos_msleep(300);
    return 0;
}



/*************************************motor state check******************************/
/*
motor limit check and deal ,touched shold been stop 
param:
    motor: motor object
*/
static void xp_rs57_limit_check(RS57 *motor){
    int sta[3]={0};

    if(motor->limit.id==0){
        return -1;
    }
    sta[0]=xp_io_read_input_pin(motor->limit.id,motor->limit.min);
    sta[1]=xp_io_read_input_pin(motor->limit.id,motor->limit.max);
    if(sta[0]==0&&sta[1]==0){
        sta[2]=-2;
        perr("RS57 motor %d max and min all limit touched !",motor->id);
    }
    else if(sta[0]==0){
        sta[2]=-1;
        perr("RS57 motor %d min limit touched !",motor->id);
    }
    else if(sta[1]==0){
        sta[2]=1;
        // perr("RS57 motor %d max limit touched !",motor->id);
    }
    else{
        sta[2]=0;
    }
    if(motor->dir==1){
        if(sta[2]==-2||sta[2]==1){
            xp_rs57_stop(motor);
        }
    }
    else if(motor->dir==-1){
        if(sta[2]==-2||sta[2]==-1){
            xp_rs57_stop(motor);
        }
    }
    else if(motor->dir==-2){
        if(sta[2]==-1||sta[2]==-2){
            println("=============limit touched============");
            if(motor->limit.status==-1||motor->limit.status==-2){
                aos_msleep(1000);
                if(xp_io_read_input_pin(motor->limit.id,motor->limit.min)==0){
                    xp_rs57_stop(motor);
                }
            }
            else{
                xp_rs57_stop(motor);
            }
        }
    }
    else if(motor->dir==0){

    }
    motor->limit.status=sta[2];
    return 0;
}


/*
motor state check thread
*/
void xp_rs57_status_check_thread(void *arg){
    RS57 *motor=(RS57 *)arg;
    int count=0;
    int sta=0;

    if(motor==NULL){
        perr("motor == NULL");
        aos_task_exit(0);
    }
    while(motor->so.id==0){
        perr("rs57 %d motor so no config ~",motor->id);
        aos_msleep(2000);
    }

    println("rs57 %d motor status check thread creat success!",motor->id);
    while(1){
        motor->status.brk=xp_io_read_input_pin(motor->so.id,motor->so.so2_brk);
        motor->status.mc_ok=xp_io_read_input_pin(motor->so.id,motor->so.so1_mc_ok);
        sta=xp_io_read_input_pin(motor->so.id,motor->so.so3_alm);
        if(sta!=motor->status.alm){
            motor->status.warn_code=xp_rs57_now_wran_get(motor);
        }
        motor->status.alm=sta;
        xp_rs57_limit_check(motor);
        count++;
        if((count%20)==1){
            println("rs57 %d motor status: brk=%d,mc_ok=%d,alm=%d",motor->id,
                                                                    motor->status.brk,
                                                                    motor->status.mc_ok,
                                                                    motor->status.alm);
            if(motor->status.alm==0){
                println("rs57 %d motor hava a error. alm=0.",motor->id);
                println("system will auto clear error.");
                aos_msleep(100);
                xp_rs57_clear(motor);
                aos_msleep(200);
            }
        }

        aos_msleep(100);
    }
}


/*************************************cmd  control********************************/
/*
modbus read data
param:
    motor: motor obj
    addr: read data start address
    data: save data buffer
    len: read data length
return:
    0=success
*/
static int xp_rs57_read(const RS57 *motor,u16 addr,u8 *data,u8 len){
    MB_data modbus={0};
    int count=5;
    int sta=0;
    u8 read_len;

    if(motor==NULL){
        perr("motor == NULL");
        return -2;
    }
    if(motor->modbus!=1){
        perr("rs57 %d motor modbus not available !",motor->id);
        return -3;
    }
    modbus.slave_id=motor->id;
    modbus.mb_fun=mb_hold_r;
    modbus.start_addr=addr;
    modbus.rwLen=len;
    modbus.resBuf=data;
    modbus.resLen=&read_len;

    while(count--){
        sta=xp_modbus_ask(motor->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta==0){
        return 0;
    }
    else{
        perr("rs57 %d motor read data fail !",motor->id);
        return -1;
    }
}


/*
modbus write data
param:
    motor: motor obj
    addr: write data address
    data: write data
    len: write data length
return:
    0=success
*/
static int xp_rs57_write(const RS57 *motor,u16 addr,u16 *data,u8 len){
    int sta=0;
    MB_data modbus={0};
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;

    if(motor==NULL){
        perr("motor == NULL");
        return -2;
    }
    if(motor->modbus!=1){
        perr("rs57 %d motor modbus not available !",motor->id);
        return -3;
    }
    modbus.slave_id=motor->id;
    modbus.mb_fun=mb_hold_w;
    modbus.start_addr=addr;
    modbus.wBuf=data;
    modbus.rwLen=len;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;

    while (count--)
    {
        sta=xp_modbus_ask(motor->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta==0){
        return 0;
    }
    else{
        perr("rs57 %d motor write data fail !",motor->id);
        return -1;
    }
}



/**************cmd config*************************/
/*
pr control config
param:
    motor:motor obj
    ctrg:ctrg mode,0=上升沿触发，1=双边沿触发
    limit:limit mode,0=软件限位无效，1=有效
    back: 上电回零，0=无效，1=有效
    ctrg_type:ctrg电平触发选择，0=边沿触发，1=电平触发(485 cmd 无效)
return:
    0=success
*/
int xp_rs57_control_config(const RS57 *motor,int ctrg,int limit,int back,int ctrg_type){
    u16 data=0;

    if(motor==NULL){
        perr("motor == NULL ");
        return -1;
    }
    data=data|(ctrg&0x01);              //bit0
    data=data|((limit&0x01)<<1);        //bit1
    data=data|((back&0x01)<<2);         //bit2
    data=data|((ctrg_type&0x01)<<4);    //bit4

    if(xp_rs57_write(motor,RS57_pr_set_addr,&data,1)!=0){
        return -2;
    }
    return 0;
}


/*
set soft limit 
param:
    motor: motor obj
    max: max limit pluse     10000=1 cycle
    min:min limit pluse
return:
    0=success
*/
int xp_rs57_limit_config(const RS57 *motor,u32 max,u32 min){
    u16 data[4];

    if(motor==NULL){
        perr("motor == NULL");
        return -1;
    }
    data[0]=max>>16;
    data[1]=max;
    data[2]=min=min>>16;
    data[3]=min;

    if(xp_rs57_write(motor,RS57_pr_max_H,data,4)==0){
        return -2;
    }
    return 0;
}


/*
back zero mode config
param:
    motor: motor obj
    dir: move direction，0=反向，1=正向
    stop_to: 回零后是否去指定位置，0=否，other=location pluse
return:
    0=success
*/
int xp_rs57_back_config(const RS57 *motor,u8 dir,u32 stop_to){
    u16 data[2];

    if(motor==NULL){
        perr("motor == NULL");
        return -1;
    }
    data[0]=0;
    data[0]=data[0]|(dir&0x01);
    if(stop_to!=0){
        data[0]=data[0]|(0x01<<1);
    }
    if(xp_rs57_write(motor,RS57_pr_back_addr,data,1)!=0){
        return -2;
    }
    if(stop_to!=0){
        data[0]=0;
        data[0]=stop_to>>16;
        data[1]=stop_to;
        if(xp_rs57_write(motor,RS57_pr_stop_to,data,2)!=0){
            return -3;
        }
    }
    return 0;
}


/*
back speed config
param:
    motor: motor obj
    speedH: high speed  r/min
    speedL: low speed   r/min
    acc_t:acc time ms/Krpm
    dec_t:acc time ms/Krpm
return:
    0=success
*/
int xp_rs57_backSpeed_config(const RS57 *motor,u16 speedH,u16 speedL,u16 acc_t,u16 dec_t){
    u16 data[4];

    if(motor==NULL){
        perr("motor == NULL");
        return -1;
    }

    data[0]=speedH;
    data[1]=speedL;
    data[2]=acc_t;
    data[3]=dec_t;

    if(xp_rs57_write(motor,RS57_pr_back_speed,data,4)!=0){
        return -2;
    }
    return 0;
}


/*
config motor route attribute
param:
    motor: motor obj
    route: route num 0~15
    attr: attribute 
return:
    0=success
*/
int xp_rs57_route_config(const RS57 *motor,const u8 route,const RS_route_attr *attr){
    u16 data[8];
    u16 addr;

    addr=RS57_route_0+(route*8);
    data[0]=attr->mode.type|((attr->mode.ins&0x01)<<4)|((attr->mode.ovlp&0x01)<<5)|
            ((attr->mode.mode&0x0f)<<6)|((attr->mode.jump_to&0x3f)<<8)|
            ((attr->mode.jump&0x0f)<<14);
    data[1]=attr->target>>16;
    data[2]=attr->target;
    data[3]=attr->speed;
    data[4]=attr->acc;
    data[5]=attr->dec;
    data[6]=attr->wait_time;
    data[7]=attr->action;

    if(xp_rs57_write(motor,addr,data,8)!=0){
        return -1;
    }
    return 0;
}


/*
config route target
param:
    motor: motor obj
    route: route num 0~15
    targe: targe value pluse
return:
    0=success
*/
int xp_rs57_route_target_updata(const RS57 *motor,const u8 route,const u32 target){
    u16 data[2];
    int addr;

    addr=RS57_route_0+(route*8)+1;
    data[0]=(target>>16);
    data[1]=target;

    println("rs57 %d motor route %d target update:%d",motor->id,
                                                    route,target);
    if(xp_rs57_write(motor,addr,data,2)!=0){
        perr("rs57 %d motor updata target %d fail !",motor->id,target);
        return -1;
    }
    return 0;
}


/*
config motor structure param
param:
    motor: motor obj
    kr: 减速机比 主动/从动
    kd: 螺距 mm
*/
void xp_rs57_structure_config(RS57 *motor,float kr,float kd){
    motor->structure.Kr=kr;
    motor->structure.Kd=kd;
}


/****************move control******************/
/*
control set
param:
    motor: motor obj
    type: move type see RS57_move_type
return:
    0=success
*/
int xp_rs57_control_set(const RS57 *motor,RS57_move_type type){
    u16 data=(u16)type;

    if(motor==NULL){
        perr("motor == NULL");
        return -1;
    }
    if(xp_rs57_write(motor,RS57_pr_ctrl_addr,&data,1)!=0){
        return -2;
    }
    return 0;
}



/*
set motor state 
param:
    motor: motor obj
    type: set type see RS57_set
return:
    0=success
*/
int xp_rs57_state_set(const RS57 *motor,const RS57_set type){
    u16 data=(u16)type;

    if(xp_rs57_write(motor,RS57_state_set,&data,1)!=0){
        return -1;
    }
    return 0;
}


/*
motor move to target
param:
    motor: motor obj
    dis: target position mm
return:
    0=success
*/
int xp_rs57_cmd_move(RS57 *motor,int dis){
    int sta=0;
    int pluse=xp_rs57_dis_to_pluse(motor,dis);

    println("rs57 %d motor move to %d mm",motor->id,dis);
    sta=xp_rs57_route_target_updata(motor,1,pluse);
    if(sta!=0){
        return -1;
    }
    xp_rs57_route_set(motor,1);
    motor->location_mm=dis;
    motor->location_pluse=pluse;
    return 0;
}



/********************************motor status************************/
/*
motor location get
param:
    motor: motor obj
return: location pluse,0xffffffff=error
*/
int xp_rs57_location_get(const RS57 *motor){
    u8 data[4];
    u32 location=0;

    if(xp_rs57_read(motor,RS57_pr_location,data,2)!=0){
        if(motor!=NULL){
            perr("rs57 %d motor get location fail !",motor->id);
        }
        return 0xffffffff;
    }
    location=(data[0]<<24)|(data[1]<<16)|(data[2]<<8)|data[3];
    return location;
}


/*
get now warn code
param:
    motor: motor obj, <0 = have a fault
return: warn code
    读取值为 0x01 ：过流；
    读取值为 0x02 ：过压；
    读取值为 0x20 ：超差；
    读取值为 0x40 ：电流采样回路故障；
    读取值为 0x80 ：锁轴故障；
    读取值为 0x100：自整定错误；
    读取值为 0x200: 存储错误；
*/
int xp_rs57_now_wran_get(const RS57 *motor){
    u8 data[2];
    u16 warn=0;

    if(xp_rs57_read(motor,RS57_now_warn,data,1)!=0){
        if(motor!=NULL){
            perr("rs57 %d motor read warn code fail !",motor->id); 
        }
        return -1;
    }
    warn=data[0]<<16|data[1];
    if(warn!=0){
        perr("rs57 %d motor warn code = 0x%x. %s",motor->id,warn,xp_rs57_warn_string(warn));
      
    }
    return warn;
}



/*
get warn string
param:
    code: warn code
return: warn string pointer
*/
static char *xp_rs57_warn_string(int code){
    static char warn[50]={0};
    
    switch (code)
    {
    case 0x01:
        sprintf(warn,"%s","过流");
        break;
    case 0x02:
        sprintf(warn,"%s","过压");
        break;
    case 0x20:
        sprintf(warn,"%s","超差");
        break;
    case 0x40:
        sprintf(warn,"%s","电流采样回路故障");
        break;
    case 0x80:
        sprintf(warn,"%s","锁轴故障");
        break;
    case 0x100:
        sprintf(warn,"自身整定错误");
        break;
    case 0x200:
        sprintf(warn,"%s","存储错误");
        break;
    default:
        sprintf(warn,"不认识代码，code=%d",code);
        break;
    }
    return warn;
}



/************** data convertion **********/
/*
convert distance to pluse
param:
    motor: motor obj
    dis: dis mm
return: pluse
*/
int xp_rs57_dis_to_pluse(const RS57 *motor,const int dis){
    return (RS57_cycle_pluse*dis*motor->structure.Kr/motor->structure.Kd);
}

int xp_rs57_angle_to_pluse(const RS57 *motor,const int angle){
	return (RS57_cycle_pluse*angle*motor->structure.Kr/360);
}



/******************************rs57 putter demo******************************/
int xp_rs57_putter_init(int port,int speed,int id){
    RS57 *motor;
    int pin[8];
    motor=xp_rs57_new(port,speed,id);
    if(motor==NULL){
        return -1;
    }
    pin[1]=0;
    pin[2]=1;
    pin[3]=2;
    pin[4]=3;
    pin[5]=19;
    pin[6]=21;
    pin[7]=22;

    xp_rs57_si_config(motor,1,pin);
    xp_rs57_so_config(motor,1,2,9,22);
    xp_rs57_structure_config(motor,50.0/30.0,10.0);
    // xp_rs57_en_set(motor,id);
}

int xp_rs57_putter_recover(){
    RS57 *motor;
    int pin[8];
    motor = xp_rs57_motor_get(7);
	if(motor == NULL){
		perr("xp_rs57_motor_get id7 failed!");
		return -1;
	}
    pin[1]=0;
    pin[2]=1;
    pin[3]=2;
    pin[4]=3;
    pin[5]=19;
    pin[6]=21;
    pin[7]=22;

    xp_rs57_si_config(motor,1,pin);
    xp_rs57_so_config(motor,1,2,9,22);
    xp_rs57_structure_config(motor,50.0/30.0,10.0);
    // xp_rs57_en_set(motor,id);
    return 0;
}

int xp_rs57_putter_move(u8 route)
{
	RS57 *motor = NULL;
	int ret = -1;

	if(route > 3){
		perr("xp_rs57_demo_move: route %d out of range!", route);
		return -1;
	}

	motor = xp_rs57_motor_get(7);
	if(motor == NULL){
		perr("xp_rs57_motor_get id7 failed!");
		return -2;
	}

	ret = xp_rs57_route_set(motor, route);
	if(ret != 0){
		println("xp_rs57_route_set %d failed! ret %d", route, ret);
		return -3;
	}

	return 0;
}

int xp_rs57_putter_checkZero(void)
{
	RS57 *motor = NULL;
	int ret = -1;

	motor = xp_rs57_motor_get(7);
	if(motor == NULL){
		perr("xp_rs57_motor_get id7 failed!");
		return -2;
	}

	ret = xp_rs57_route_set(motor, 3);
	if(ret != 0){
		println("xp_rs57_route_set 3 failed! ret %d", ret);
		return -3;
	}


	return 0;
}

/*clear putter warn
*/
int xp_clear_putter_warn(void){
    RS57 *motor = NULL;

    motor = xp_rs57_motor_get(7);
    if(motor == NULL){
		perr("xp_rs57_motor_get id7 failed!");
		return -2;
	}
    
    if(xp_rs57_clear(motor)!=0)
    {
        println("putter warn clear faild!!!!!!!!");
        return -1;
    }
    
    println("putter warn clear success");
    return 0;
}

bool xp_read_putter_mc_ok_flag(void){
    return xp_rs57_motor_get(7)->status.mc_ok;
}

// bool xp_read_swing_mc_ok_flag(void){
//     return xp_rs57_motor_get(8)->status.mc_ok;
// }

bool xp_read_putter_alm_flag(void){
    return xp_rs57_motor_get(7)->status.alm;
}


/*************************mirror demo*****************************/
// static int mirror_id=14,15;     //mirror driver modbus id

/*
mirror demo init
param:
    port: uart port
    speed: uart speed
return:
    0=success
*/
int xp_rs57_mirror_init(int port,int speed,int id){
    RS57 *motor;
    int pin[8];
    int ret = -1;

    motor=xp_rs57_new(port,speed,id);   //无雷达侧
    if(motor==NULL){
        return -1;
    }
    pin[1]=0;
    pin[2]=0;
    pin[3]=1;
    pin[4]=2;
    pin[5]=3;
    pin[6]=4;
    pin[7]=5;

    xp_rs57_si_config(motor,2,pin);
    xp_rs57_so_config(motor,2,0,1,2);
    xp_rs57_structure_config(motor,30,1);
    //xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,2,12,0);
    xp_rs57_limit_io_config(motor,2,8,10);

    motor=xp_rs57_new(port,speed,id+1); //雷达侧
    if(motor==NULL){
        return -2;
    }
    pin[1]=0;
    pin[2]=6;
    pin[3]=7;
    pin[4]=8;
    pin[5]=9;
    pin[6]=10;
    pin[7]=11;

    xp_rs57_si_config(motor,2,pin);
    xp_rs57_so_config(motor,2,3,4,5);
    xp_rs57_structure_config(motor,30,1);
    //xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,2,13,0);
    xp_rs57_limit_io_config(motor,2,9,11);

    return 0;
}

int xp_rs57_mirror_recover(){
    RS57 *motor;
    int pin[8];

    motor = xp_rs57_motor_get(14);
	if(motor == NULL){
		perr("xp_rs57_motor_get id14 failed!");
		return -1;
	}
    pin[1]=0;
    pin[2]=0;
    pin[3]=1;
    pin[4]=2;
    pin[5]=3;
    pin[6]=4;
    pin[7]=5;

    xp_rs57_si_config(motor,2,pin);
    xp_rs57_so_config(motor,2,0,1,2);
    xp_rs57_structure_config(motor,30,1);
    //xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,2,12,0);
    xp_rs57_limit_io_config(motor,2,8,10);

    motor = xp_rs57_motor_get(15);
	if(motor == NULL){
		perr("xp_rs57_motor_get id15 failed!");
		return -2;
	}
    pin[1]=0;
    pin[2]=6;
    pin[3]=7;
    pin[4]=8;
    pin[5]=9;
    pin[6]=10;
    pin[7]=11;

    xp_rs57_si_config(motor,2,pin);
    xp_rs57_so_config(motor,2,3,4,5);
    xp_rs57_structure_config(motor,30,1);
    //xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,2,13,0);
    xp_rs57_limit_io_config(motor,2,9,11);

    return 0;
}


/*
mirror demo route move 
param:
    route: route number, 0=origin,1=any point,2=max point,3=check zero
return:
    0=success
*/
int xp_rs57_mirror_route_set(int id,u8 route){
    RS57 *motor=NULL;
    int ret=0;

    if(route>3){
        perr("rs57 mirror demo route set fail, route =%d.",route);
        return -1;
    }
    motor=xp_rs57_motor_get(id);
    if(motor==NULL){
        perr("rs57 mirror demo get motor fail.");
        return -2;
    }
    ret=xp_rs57_route_set(motor,route);
    if(ret!=0){
        perr("rs57 mirror demo route set fail, ret=%d.",ret);
        return -3;
    }
    return 0;
}


/*
mirror demo move
param:
    angle: move to angle
return:
    0=success
*/
int xp_rs57_mirror_move(int id,int angle){
    RS57 *motor=NULL;
    int sta=0;

    if(angle<0||angle>90){
        perr("mirror move angle fail. angle=%d.",angle);
        return -1;
    }
    motor=xp_rs57_motor_get(id);
    if(motor==NULL){
        perr("mirror motor get fail.");
        return -2;
    }
    int a=xp_rs57_angle_to_pluse(motor,angle);
    sta=xp_rs57_route_target_updata(motor,1,a);
    if(sta!=0){
        perr("mirror move target update fail. sta=%d.",sta);
        return -3;
    }
    sta=xp_rs57_route_set(motor,1);
    if(sta!=0){
        perr("mirror move, set route 1 fail .");
        return -4;
    }
    println("rs57 mirror move to: %d degree.",angle);
    return 0;
}


/*
mirror locker set
param:
    id:modbus id
    sta: open status,1=open,0=close
return:
    0=success
*/
int xp_mirror_locker_set(int id,bool sta){
    RS57 *motor=NULL;
    
    motor=xp_rs57_motor_get(id);
    if(motor==NULL){
        return -1;
    }
    return xp_rs57_hold_set(motor,sta);
}


/*
mirrors locker set
param:
    id:modbus start id
    sta: open status,1=open,0=close
return:
    0=success
*/
int xp_mirrors_locker_set(int id,bool sta){
    if(xp_mirror_locker_set(id,sta)!=0){
        perr("mirror %d locker set as %d fail.",id,sta);
        return -1;
    }
    if(xp_mirror_locker_set(id+1,sta)!=0){
        perr("mirror %d locker set as %d fail.",id+1,sta);
        return -2;
    }
    return 0;
}

/*
mirror enable set
param:
    id:modbus id
    sta: open status,1=open,0=close
return:
    0=success
*/
int xp_mirror_enable_set(int id,bool sta){
    RS57 *motor=NULL;
    
    motor=xp_rs57_motor_get(id);
    if(motor==NULL){
        return -1;
    }
    return xp_rs57_en_set(motor,sta);
}

/*
mirrors enable set
param:
    id:modbus start id
    sta: open status,1=open,0=close
return:
    0=success
*/
int xp_mirrors_enable_set(int id,bool sta){
    if(xp_mirror_enable_set(id,sta)!=0){
        perr("mirror %d enable set as %d fail.",id,sta);
        return -1;
    }
    if(xp_mirror_enable_set(id+1,sta)!=0){
        perr("mirror %d enable set as %d fail.",id+1,sta);
        return -2;
    }
    return 0;
}

/*
check mirrors is moving
return:
    1=moving, 0=NOT moving
*/
int xp_mirrors_is_moving(void){
	RS57 *motor[2] = {NULL, NULL};
	uint8_t i = 0;

	for (i = 0; i < 2; i++){
		motor[i] = xp_rs57_motor_get(14+i);
		if(motor[i] == NULL){
			perr("mirror motor%d id%d get fail.", i, 14+i);
			return -1;
		}

		//任一反光镜在运动中则返回1
		if(1 == motor[i]->status.mc_ok){
			return 1;
		}
	}

	return 0;
}

/*
mirrors route move
param:
    id: modbus start id
    route: route number
return:
    0=success
*/
int xp_mirrors_route_move(int id,u8 route){
	uint32_t wait_cnt = 0;
	uint32_t retry_cnt = 0;

RETRY:
    //使能电机，使抱闸打开
    xp_mirrors_enable_set(id,1);

    //打开抱闸
    xp_mirrors_locker_set(id,1);
    aos_msleep(1000);

    if(xp_rs57_mirror_route_set(id,route)!=0){
        perr("mirror %d route %d move fail.",id,route);
        return -1;
    }
    if(xp_rs57_mirror_route_set(id+1,route)!=0){
        perr("mirror %d route %d move fail.",id+1,route);
        return -1;
    }

    if(3 == route){
        g_last_mirrors_checkzero_time = aos_now_ms();
    }else if(0 == route){
        g_last_mirrors_close_time = aos_now_ms();
    }

	aos_msleep(1000);
	wait_cnt = 0;
	while(1 == xp_mirrors_is_moving()){
		aos_msleep(200);
		wait_cnt++;
		if(wait_cnt > 50){
			//超过10s未运动到指定位置
			retry_cnt++;
			if(retry_cnt > 3){
				perr("mirrors move to target route %d retry %d loops failed! Exit!", route, retry_cnt);
				return -2;
			}else{
				perr("mirrors move to target route %d timeout! retry_cnt %d...", route, retry_cnt);
				goto RETRY;
			}
		}
	}
	
	//关闭抱闸
	xp_mirrors_locker_set(id,0);

	//去使能电机，使抱闸抱上
	xp_mirrors_enable_set(id,0);

    return 0;
}

/*
mirrors move to the angle
param:
    id: modbus start id
    route: route number
return:
    0=success
*/
int xp_mirrors_move(int id,int angle){
	RS57 *motor[2] = {NULL, NULL};
	uint8_t i = 0;
	uint16_t wait_cnt = 0;

	//打开抱闸
	xp_mirrors_locker_set(id,1);

	for (i = 0; i < 2; i++){
		motor[i] = xp_rs57_motor_get(id+i);
		if(motor[i] == NULL){
		    perr("mirror motor%d id%d get fail.", i, id+i);
		    return -1;
		}

		if(xp_rs57_mirror_move(id+i, angle)!=0){
		    perr("mirror %d move angle %d degree fail.",id+i,angle);
		    return -2;
		}
	}

    return 0;
}

/*clear mirrors warn
*/
int xp_mirrors_clear_warn(void){
    RS57 *motor = NULL;
	int i = 0;

	for(i = 0; i < 2; i++){
	    motor = xp_rs57_motor_get(14+i);
	    if(motor == NULL){
			perr("xp_rs57_motor_get id%d failed!", 14+i);
			return -2;
		}
	    
	    if(xp_rs57_clear(motor)!=0)
	    {
	        println("mirror_%d warn clear faild!!!!!!!!", i);
	        return -1;
	    }
	    
	    println("mirror_%d warn clear success", i);
	}
    return 0;
}

int xp_mirrors_checkZero(void)
{
	int ret = -1;

	xp_mirrors_clear_warn();
    aos_msleep(200);
	ret = xp_mirrors_route_move(14, 3);
    if(ret < 0){
        println("xp_mirrors_route_move 3 failed! ret %d", ret);
        return -1;
    }

	return 0;
}

MIRROR_state xp_mirrors_get_state(void)
{
    if(xp_io_read_input_pin(2,6)==0&&xp_io_read_input_pin(2,7)==0){
        mirrors_state=MIRROR_close;
    }
    /*else if(xp_io_read_input_pin(2,8)==0&&xp_io_read_input_pin(2,9)==0){
        mirrors_state=MIRROR_open;
    }*/
    else{
        mirrors_state=MIRROR_unknown;
    }
	return mirrors_state;
}


/*******************************************debug********************************/
//rs57 debug
static int xp_rs57_debug(char *type,char *fun,char *param){
    char buf[10];
    char *p,*f;
    int id=0;
    RS57 *motor;
    if(strstr(type,"rs57-")!=type)return 0;
    strncpy(buf,type,9);
    p=strtok_r(buf,"-",&f);
    id=atoi(f);
    motor=xp_rs57_motor_get(id);
    if(motor==NULL){
        perr("no rs57 %d motor obj",id);
        return 1;
    }
    if(strcmp(fun,"en")==0){
        id=atoi(param);
        xp_rs57_en_set(motor,id);
    }
    else if(strcmp(fun,"run_addr")==0){
        id=atoi(param);
        xp_rs57_route_set(motor,id);
    }
    else if(strcmp(fun,"clear")==0){
        xp_rs57_clear(motor);
    }
    else if(strcmp(fun,"origin")==0){
        xp_rs57_origin_go(motor);
    }
    else if(strcmp(fun,"stop")==0){
        xp_rs57_stop(motor);
    }
    else if(strcmp(fun,"alm_get")==0){
        println("rs57 %d motor alm: %d",motor->id,xp_rs57_now_wran_get(motor));
    }
    else if(strcmp(fun,"target1")==0){
        xp_rs57_route_target_updata(motor,1,xp_rs57_dis_to_pluse(motor,atoi(param)));
    }
    else if(strcmp(fun,"move")==0){
        xp_rs57_cmd_move(motor,atoi(param));
    }
    else if(strcmp(fun,"mirror_route")==0){
        xp_rs57_hold_set(motor,1);
        xp_rs57_mirror_route_set(id,atoi(param));
    }
    else if(strcmp(fun,"mirror_move")==0){
        xp_rs57_hold_set(motor,1);
        xp_rs57_mirror_move(id,atoi(param));
    }
    else if(strcmp(fun,"mirrors_route")==0){
        xp_mirrors_route_move(id,atoi(param));
    }
    else if(strcmp(fun,"mirrors_move")==0){
        xp_mirrors_move(id,atoi(param));
    }
    else if(strcmp(fun,"mirrors_locker")==0){
        xp_mirrors_locker_set(id,atoi(param));
    }
    else{
        return 0;
    }
    return 1;
}

#define PIN_OPEN_LIMIT_L             1
#define PIN_OPEN_LIMIT_R             2
#define PIN_CLOSE_LIMIT_L            3
#define PIN_CLOSE_LIMIT_R            4

#define PIN_OPEN_JOG_L               5
#define PIN_OPEN_JOG_R               6
#define PIN_CLOSE_JOG_L              7
#define PIN_CLOSE_JOG_R              8

#define MOVE_OVER_TIME              5000

enum{
    IO_LU = 0x01,
    IO_RU = 0x02,
    IO_LD = 0x04,
    IO_RD = 0x08,
};


uint8_t xp_mirror_get_io_state(void)
{
    uint8_t ret = 0;
    if (xp_io_read_input_pin(2, PIN_OPEN_LIMIT_L) == 0){
        ret |= IO_LU;
    }
    if (xp_io_read_input_pin(2, PIN_OPEN_LIMIT_R) == 0){
        ret |= IO_RU;
    }
    if (xp_io_read_input_pin(2, PIN_CLOSE_LIMIT_L) == 0){
        ret |= IO_LD;
    }
    if (xp_io_read_input_pin(2, PIN_CLOSE_LIMIT_R) == 0){
        ret |= IO_RD;
    }
    return ret;
}
//驱动器配置成开环,正反限位由限位传感器自动触发
bool xp_mirrors_move_by_jog(bool isOpen)
{
    uint8_t limitIoState = xp_mirror_get_io_state();

    /*1. 首先如果检测到左右镜子上下限位皆触发,则说明有限位传感器损坏*/
    if ( (limitIoState & IO_LU) && (limitIoState & IO_LD) ){
        println("Mirrors left limit switch has bad\r\n");
        return false;
    }

    if ( (limitIoState & IO_RU) && (limitIoState & IO_RD) ){
        println("Mirrors right limit switch has bad\r\n");
        return false;
    }

    uint32_t timeStamp = aos_now_ms();
    bool isComplite = false;

    if (isOpen){
        /*1. 如果上限位此时已触发,则忽略*/
        if ( !(limitIoState & IO_LU) ){
            xp_io_write_pin(2, PIN_OPEN_JOG_L, 0);
        }
        if ( !(limitIoState & IO_RU) ){
            xp_io_write_pin(2, PIN_OPEN_JOG_R, 0);
        }

        while(aos_now_ms() - timeStamp < MOVE_OVER_TIME){
            limitIoState = xp_mirror_get_io_state();
            
            if ( (limitIoState & IO_LU) ){
                xp_io_write_pin(2, PIN_OPEN_JOG_L, 1);
            }
            if ( (limitIoState & IO_RU) ){
                xp_io_write_pin(2, PIN_OPEN_JOG_R, 1);
            }

            if ( (limitIoState & IO_LU) && (limitIoState & IO_RU) ){
                isComplite = true;
                break;
            }
            aos_msleep(10);
        }

        //释放所有IO
        xp_io_write_pin(2, PIN_OPEN_JOG_L, 1);
        xp_io_write_pin(2, PIN_OPEN_JOG_R, 1);
        xp_io_write_pin(2, PIN_CLOSE_JOG_L, 1);
        xp_io_write_pin(2, PIN_CLOSE_JOG_R, 1);
    }else{

        if ( !(limitIoState & IO_LD) ){
            xp_io_write_pin(2, PIN_CLOSE_JOG_L, 0);
        }
        if ( !(limitIoState & IO_RD) ){
            xp_io_write_pin(2, PIN_CLOSE_JOG_R, 0);
        }

        while(aos_now_ms() - timeStamp < MOVE_OVER_TIME){
            limitIoState = xp_mirror_get_io_state();
            
            if ( (limitIoState & IO_LD) ){
                xp_io_write_pin(2, PIN_CLOSE_JOG_L, 1);
            }
            if ( (limitIoState & IO_RD) ){
                xp_io_write_pin(2, PIN_CLOSE_JOG_R, 1);
            }

            if ( (limitIoState & IO_LD) && (limitIoState & IO_RD) ){
                isComplite = true;
                break;
            }
            aos_msleep(10);
        }

        //释放所有IO
        xp_io_write_pin(2, PIN_OPEN_JOG_L, 1);
        xp_io_write_pin(2, PIN_OPEN_JOG_R, 1);
        xp_io_write_pin(2, PIN_CLOSE_JOG_L, 1);
        xp_io_write_pin(2, PIN_CLOSE_JOG_R, 1);
    }

    return isComplite;
}

//end of the file
